#!/usr/bin/env python3

import rospy

from geometry_msgs.msg import Twist
from std_msgs.msg import String
import sys, select, termios, tty
from control_msgs.msg import control_msg

msg = """
Control Your Turtlebot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
s:    switch control mode
anything else : stop smoothly

CTRL-C to quit
"""

moveBindings = {
        'i':(1,0),
        'o':(1,-1),
        'j':(0,1),
        'l':(0,-1),
        'u':(1,1),
        ',':(-1,0),
        '.':(-1,1),
        'm':(-1,-1),
           }

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
          }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.05)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = 0.2
turn  = 0.2

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('turtlebot_teleop')
    pub = rospy.Publisher('/cmd_vel_manual', Twist, queue_size=5)

    pub_control = rospy.Publisher("/control", control_msg, queue_size=10)

    x = 0
    th = 0
    status = 0
    count = 0
    acc = 0.1
    target_speed  = 0
    target_turn   = 0
    control_speed = 0
    control_turn  = 0

    stop = False
    lift = 0

    try:
        print(vels(speed,turn))
        
        while not rospy.is_shutdown():
            # lift = 0
            key = getKey()
            if key == "s":
                stop = stop ^ True
                msgs = control_msg()
                if stop:
                    msgs.speed = 0.0
                    msgs.angle_speed = 0.0
                    speed = 0.2
                    turn  = 0.2
                    lift = 0
                rospy.loginfo(stop)
                pub_control.publish(msgs)
            else:
                if key in moveBindings.keys():
                    x     = moveBindings[key][0]
                    th    = moveBindings[key][1]
                    count = 0
                    lift = 0
                elif key in speedBindings.keys():
                    speed = speed * speedBindings[key][0]
                    turn  = turn * speedBindings[key][1]
                    count = 0
                    lift = 0

                    if (speed > 1.0):
                        speed = 1.0
                    if (turn > 0.5):
                        trun = 0.5
                    if (speed < 0.05):
                        speed = 0.05
                    if (turn < 0.01):
                        turn = 0.01

                    print(vels(speed,turn))
                    status = (status + 1) % 15
                elif key == ' ' or key == 'k' :
                    x = 0
                    th = 0
                    lift = 0
                    control_speed = 0
                    control_turn  = 0
                elif key == '9' or key == '0':
                    if key == '9':
                        lift = 1
                    else:
                        lift = -1
                else:
                    count = count + 1
                    if count > 10:
                        x = 0
                        th = 0
                        lift = 0
                    if (key == '\x03'):
                        break
                
                target_speed = speed * x
                target_turn  = turn  * th

                rospy.loginfo("key: %s target_speed: %f taraget_v: %f  lift: %d", key, speed, target_speed, lift)

                if target_speed > control_speed:
                    control_speed = min( target_speed, control_speed + 0.01 )
                elif target_speed < control_speed:
                    control_speed = max( target_speed, control_speed - 0.01 )
                else:
                    control_speed = target_speed

                if target_turn > control_turn:
                    control_turn = min( target_turn, control_turn + 0.05 )
                elif target_turn < control_turn:
                    control_turn = max( target_turn, control_turn - 0.05 )
                else:
                    control_turn = target_turn

                msgs = control_msg()

                if (abs(control_speed) < 0.05 and abs(control_turn) > 0.01):
                    msgs.running_type = 1
                else:
                    msgs.running_type = 0

                if (stop == False):
                    msgs.speed = control_speed
                    msgs.angle_speed = control_turn

                else:
                    msgs.angle_speed = 0.0
                    msgs.speed       = 0.0

                msgs.header.stamp = rospy.Time()
                pub_control.publish(msgs)

                # if stop:
                    # pub.publish(twist)

                #print("loop: {0}".format(count))
                # print("x: {0}, speed: {1}, target: {2}, control_speed: {3}".format(x, speed, target_speed, control_speed))
                # print("target: vx: {0}, wz: {1} control_speed: vx: {2}, wz: {3}, x: {4}".format(target_speed, target_turn, control_speed, control_turn, x))
                #print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z))

    except Exception as e:
        print(e)

    finally:
        msgs = control_msg()
        msgs.angle_speed = 0.0
        msgs.speed       = 0.0
        pub_control.publish(msgs)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
